Multivariate Taylor Series Expansion

Solution to the Previous Quiz

Solution

h(x) \approx h(\mu) + \frac{\partial h(\mu)}{\partial x}(x-\mu) = \arctan(\mu) + \frac{1}{1+\mu^2}(x-\mu)

In our example \mu=0 , therefore:

h(x) \approx \arctan(0) + \frac{1}{1+0}(x-0) = x

So, the function, h(x) = \arctan(x) , will be approximated by a line:
h(x) \approx x .

And now, let's keep going!

Multivariate Taylor Series

Now that you’ve seen how to do a Taylor series expansion with a one-dimensional equation, we’ll need to look at the Taylor series expansion for multi-dimensional equations. Recall from the Radar Measurements lecture that the h function is composed of three equations that show how the predicted state, x' = (p_x', p_y', v_x', v_y')^T , is mapped into the measurement space, z = (\rho, \varphi, \dot{\rho})^T :

h(x')= \begin{pmatrix} \sqrt{ p{'}_x^2 + p{'}_y^2 }\\ \arctan(p_y' / p_x')\\ \frac{p_x' v_x' + p_y' v_y'}{\sqrt{p{'}_x^2 + p{'}_{y}^2}} \end{pmatrix}

These are multi-dimensional equations, so we will need to use a multi-dimensional Taylor series expansion to make a linear approximation of the h function. Here is a general formula for the multi-dimensional Taylor series expansion:

T(x) = f(a) + (x - a)^TDf(a) + \frac 1{2!}(x-a)^T{D^2f(a)}(x - a) + ...

where Df(a) is called the Jacobian matrix and D^2f(a) is called the Hessian matrix . They represent first order and second order derivatives of multi-dimensional equations. A full Taylor series expansion would include higher order terms as well for the third order derivatives, fourth order derivatives, and so on.

Notice the similarities between the multi-dimensional Taylor series expansion and the one-dimensional Taylor series expansion:

T(x) = f(a) + \frac{f'(a)}{1!}(x - a) + \frac{f''(a)}{2!}(x-a)^2 + \frac{f'''(a)}{3!}(x - a)^3 + ...

To derive a linear approximation for the h function, we will only keep the expansion up to the Jacobian matrix Df(a) . We will ignore the Hessian matrix D^2f(a) and other higher order terms. Assuming (x - a) is small, (x - a)^2 or the multi-dimensional equivalent (x-a)^T (x - a) will be even smaller; the extended Kalman filter we'll be using assumes that higher order terms beyond the Jacobian are negligible.

Let's first calculate the Jacobian matrix for the extended Kalman filter. Then we'll show the difference between the Kalman filter equations and the extended Kalman filter equations.